clear;
clc;

%% Power Flow calculation
%% Case x bus data 
% *********************************************
realmpc = realcase39;
Y=Ybus_new(realmpc);                 % 计算节点导纳矩阵
result=runpf(realmpc);               % 求解该系统潮流
f0=60;
w_syn=2*pi*f0; % 同步转速
Xd=[0.006; 0.0697; 0.0531; 0.0436; 0.132; 0.05; 0.049; 0.057; 0.057; 0.031]; 
H=[500; 30.3; 35.8; 28.6; 26; 34.8; 26.4; 24.3; 34.5; 42];  % 发电机惯性常数
R=zeros(length(Xd), 1);
D=[0; 0;0 ;0; 0; 0; 0; 0; 0; 0];  % 阻尼系数
% Xd=[0.06080; 0.11980; 0.18130];
% R=[0;0;0];
% H=[23.64; 6.4; 3.010];
% D=[0.0125;0.0034;0.0016];
% *********************************************

D=D/w_syn; % 阻尼系数/同步转速
M=2*H/w_syn; % 文章中表达方法是w0/2H

Vmag=result.bus(:, 8);              % Pu voltage magnitude of each buses 
Vph=result.bus(:, 9);               % angle in degree
V=Vmag.*exp(1j*Vph*pi/180);         % 系统电压
P_jQ=conj(V).*(Y*V);                % Net Power at each node 网络各节点潮流
S=conj(P_jQ);                       % 网络节点潮流
S=S/realmpc.baseMVA;                            % 标幺
Sg=result.gen(:, 2)+1j*result.gen(:, 3);    % 发电机输出功率
Sg=Sg/realmpc.baseMVA;                          % 标幺
gen_bus=result.gen(:, 1);  

%% 下面进入暂态分析
%% calculate Y22
Y22=diag(1./(1j*Xd));                   % Y22是Xd各元素组成的对角矩阵，应该是发电机的阻抗

%% Calculation of Y11
SL=result.bus(:, 3)+1j*result.bus(:, 4);    % 计算结果中节点功率
SL=SL/realmpc.baseMVA;                          % 标幺
YL=conj(SL)./(abs(V).^2);                   % 求节点等效导纳
Y11=Y+diag(YL);                             % 导纳矩阵加上负荷
Y11(gen_bus, gen_bus)=Y11(gen_bus, gen_bus)+Y22;    % 在自导纳上加上发电机的导纳

%% Calculation of Y12 and Y21
% Calculation of Y12
Y12=zeros(length(result.bus(:,1)), length(result.gen(:,1)));    % 节点数×发电机数
%Y12(gen_bus, gen_bus)=Y12(gen_bus, gen_bus)-Y22; 

for i=1:length(result.bus(:,1))                 % 遍历bus
    for k=1:length(result.gen(:,1))             % 遍历gen
        q=result.gen(k,1);                      % 储存q为gen所在的节点编号
        if i==q                                 % 对所有gen在的节点
            Y12(q,k)=-1/(R(k)+Xd(k)*1j);        % 计算Y12为发电机的导纳
        end 
    end 
end 

Y21=transpose(Y12);                             % 转置
%% Calculation of reduced matrix before fault 计算故障前的简化矩阵
Ybf=Y22-Y21*inv(Y11)*Y12 ;                      % Ybf=Y22-Y21*Y11逆*Y12

% Bus Reconstruction matrix 节点重构矩阵
RV(:, :, 1)=-inv(Y11)*Y12;

%% Enter fault here to calculate the afterfault and during fault reduced 故障点位置
% matrices 
f11=5;
F=[5 6];
f1=F(1);
f2=F(2);
%% during fault 计算故障中的导纳矩阵
Y11df=Y11;                                      % 导纳矩阵加负荷
Y11df(f11, :)=[];                               % 对应行去掉
Y11df(:,f11)=[];                                % 对应列去掉
Y12df=Y12;                                      % Y12计算
Y12df(f11, :)=[];                               % 对应行去掉
Y21df=transpose(Y12df);                         % 取转置
% during fault reduced matrics
Ydf=Y22-Y21df*inv(Y11df)*Y12df;                 % 重新计算Ydf

RV(:, :, 2)=zeros(size(RV(:, :, 1)));
RV(1:end-1, :, 2)=RV(1:end-1, :, 2)-inv(Y11df)*Y12df;


%% afterfault Y11                               % 故障后的Y11，此时线路切除
 Y11after=Y11;                                  % 故障后的Y11中的元素置零
 Y11after(f1,f2)=0;
 Y11after(f2,f1)=0;
for i=1:length(result.branch(:,1))  % 遍历所有支路
    if (f1==result.branch(i,1)&& f2==result.branch(i,2))||(f2==result.branch(i,1)&& f1==result.branch(i,2))     % 支路的始末端为故障的始末端
        Y11after(f1,f1)=Y11after(f1,f1)-result.branch(i,5)*1j/2-1/(result.branch(i,3)+result.branch(i,4)*1j);   
        Y11after(f2,f2)=Y11after(f2,f2)-result.branch(i,5)*1j/2-1/(result.branch(i,3)+result.branch(i,4)*1j);   % 导纳矩阵重新计算（支路断开的计算方法）
    end
end 

% Afterfault reduced matrix is 
Yaf=Y22-Y21*inv(Y11after)*Y12 ;                 % 故障后
%RV_af=-inv(Y11after)*Y12 ; 

RV(:, :, 3)=-inv(Y11after)*Y12; 
%% Initialization 初始化系统
deltt=0.0005;   % 步长
t_SW=1;         % 故障开始时刻
t_FC=1.0333;    % 故障结束时刻
t_max=10;       % 仿真结束时间

Ig=conj(Sg./V(1:length(result.gen(:, 1)))); % 发电机电流
E0=V(gen_bus)+Ig.*(R+1j*Xd);                % 机端电压
E_abs=abs(E0);

I0=Ybf*E0;                                  % I0为初始机端电压
delta0=angle(E0)*180/pi;                    % 发电机初始电压相角 没有用到
w0=zeros(length(Xd), 1);                    % w0都设为0
X_0=[angle(E0); w0];                        % X_0为初始电压相角和w0的集合，w0为转速，初值设为0

% Initialize power injection 
PG0=real(E0.*conj(I0));         % 发电机有功输入
PM=PG0;                         % 代替了PG0不知道为什么要多这一步，PG0后面都没用到
QG0=imag(E0.*conj(I0));         % 发电机无功输入

YBUS(:, :, 1)=Ybf;              % 定义YBUS
YBUS(:, :, 2)=Ydf; 
YBUS(:, :, 3)=Yaf;              % 给三维数组分别赋故障前中后三个时间的导纳矩阵
n=length(gen_bus);              % n个发电机节点
s=length(result.bus(:, 1));     % s个节点


%% Estimated State: 对系统的状态估计
% Number of states and measurements 状态量和测量量的个数
ns=2*n;             % 2n个状态量
nm=2*n+2*s;         % 2n+2s个测量量 n个发电机的有功无功 n个节点的电压幅值相角

% Covariance Matrix
sig=1e-2;           % 量测方差
P=sig^2*eye(ns);  % Error covariance matrix 协方差矩阵
Q=sig^2*eye(ns); % system noise covariance matrix 系统模型误差
R=sig^2*eye(nm); % measurment noise covariance matrix 测量噪声方差

X_true = [];   % Initial statel 
Z_true = [];
Z = [];
% 为了加入量测噪声添加的两个参数
Z_error = zeros(1,nm);
Z_Cov = 1e-3*rand(1,nm);

% constant values 

RMSE=[]; % 对比标准

%Extended Kalman Filter (EKF) ALgorithm 使用扩展卡尔曼滤波算法求解
for k=0:deltt:t_max     % 仿真出从0到结束的模型
    % Ybus and reconstruction matrix accodring to the requirement 给ps赋值以定下该时刻系统的运行状态
    if k<t_SW
        ps=1;
    elseif (t_SW<k)&&(k<=t_FC)
        ps=2;  
    else 
        ps=3; 
    end  
    
    Ybusm = YBUS(:,:,ps);           % 取出对应状态下的Ybus
    RVm=RV(:, :, ps);               % 对应状态下的RV
    
    [~, X] = ode45(@(t,x) dynamic_system(t,x,M,D,Ybusm,E_abs,PM,n),[k k+deltt],X_0);        % 用ode45求解器求解动态系统
    
    X_0=transpose(X(end, :));       % 对X最后一行做转置，得到一列，X是上面求解器得到的结果，应该是一个系统的状态值
    X_true=[X_true X_0];              % X_mes每次都在右边加上真值
    
    %determine the measurements 计算得到量测的值
    E1=E_abs.*exp(1j*X_0(1:n));     % 发现Eabs是不变的
    I1=Ybusm*E1;        % 当前状态下的电流向量
    PG=real(E1.*conj(I1));  % 输出有功
    QG=imag(E1.*conj(I1));  % 输出无功
    Vmag=abs(RVm*E1);       % 幅值
    Vangle=angle(RVm*E1);   % 相角
    % ！！！！！！！！！下面这段代码是为了让功角连续！！！！！！！！！！！
    if ~isempty(Z)
        Vangle = fix((Z(2*n+s+1:2*n+2*s)-Vangle+pi)/(2*pi))*2*pi+Vangle;
    end
    % ！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！
    Z=[PG; QG; Vmag; Vangle];   % 量测量对应的真值(这里似乎完全没有考虑量测误差)
    Z_true = [Z_true Z];
end

%% 添加误差
Z_times = zeros(size(Z_true,1),size(Z_true,2));
for i = 1:size(Z_true,2)
    Z_times(:,i) = transpose(1+normrnd(Z_error,Z_Cov));
end

% Z_measure = Z_true;
Z_measure = Z_true.*Z_times;
Z_times(2*n+s+1:2*n+2*s,:) = Z_times(2*n+s+1:2*n+2*s,:)-1;
Z_measure(2*n+s+1:2*n+2*s,:) = Z_true(2*n+s+1:2*n+2*s,:)+(2*pi*Z_times(2*n+s+1:2*n+2*s,:));
% Z_measure = Z_true;

% ！！！！！！！！！！由于上面所用代码让功角连续了，所以这部分代码不需要使用！！！！！！！！！！！！
% ！！！！当上面代码取消使用时，这部分代码有必要使用
% tmp = Z_measure(end-s+1:end,:);
% flag = Z_measure(end-s+1:end,:)>pi;
% tmp(flag) = pi;
% flag = Z_measure(end-s+1:end,:)<(-1*pi);
% tmp(flag) = (-1*pi);
% Z_measure(end-s+1:end,:) = tmp;
% ！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！

%% 保存计算＆量测数据
save('X_true.mat','X_true');
save('Z_true.mat','Z_true');
save('Z_measure.mat','Z_measure');

%% Plots
t= (0:deltt:t_max);
% for i=1:1:n
%     figure(i)
%     subplot(2,1,1)
%     plot(t,X_true(i, :), 'linewidth', 1.5)
%     hold on 
%     grid on
%     ylabel(sprintf('功角_{%d}', i), 'fontsize', 12)
%     xlabel('时间(s)', 'fontsize', 15); 
%     title('功角真实值 \delta', 'fontsize', 12)
%     legend(sprintf('功角_{%d, Actual} ',i)); 
% 
%     subplot(2,1,2)
%     plot(t,X_true(i+n, :), 'linewidth', 1.5)
%     hold on 
%     grid on
%     ylabel(sprintf('转速_{%d}', i), 'fontsize', 12)
%     xlabel('时间(s)', 'fontsize', 15); 
%     title('转速真实值 \omega', 'fontsize', 12)
%     legend(sprintf('Speed_{%d, Actual} ',i), sprintf('Speed_{%d, EKF}', i));
% end
% 
for i = 1:n
    figure(i+n)
    subplot(2,1,1)
    plot(t,Z_true(i, :), 'linewidth', 1.5)
    hold on 
    plot(t,Z_measure(i, :), 'linestyle', '--', 'color', 'b', 'linewidth', 2)
    grid on
    ylabel(sprintf('发电机输出有功_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('发电机输出有功功率的真实值与测量值 \delta', 'fontsize', 12)
    legend(sprintf('发电机输出有功真实值{%d, Actual} ',i), sprintf('发电机输出有功测量值{%d, EKF}', i));

    subplot(2,1,2)
    plot(t,Z_true(i+n, :), 'linewidth', 1.5)
    hold on 
    plot(t,Z_measure(i+n, :), 'linestyle', '--', 'color', 'b', 'linewidth', 2)
    grid on
    ylabel(sprintf('发电机输出无功_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('发电机输出无功功率的真实值与测量值 \delta', 'fontsize', 12)
    legend(sprintf('发电机输出无功真实值{%d, Actual} ',i), sprintf('发电机输出无功测量值{%d, EKF}', i));

end

for i = 1:s
    figure(i+2*n)
    subplot(2,1,1)
    plot(t,Z_true(i+2*n, :), 'linewidth', 1.5)
    hold on 
    plot(t,Z_measure(i+2*n, :), 'linestyle', '--', 'color', 'b', 'linewidth', 2)
    grid on
    ylabel(sprintf('节点电压幅值_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('节点电压真实值与测量值 \delta', 'fontsize', 12)
    legend(sprintf('节点电压真实值{%d, Actual} ',i), sprintf('节点电压测量值{%d, EKF}', i));

    subplot(2,1,2)
    plot(t,Z_true(i+2*n+s, :), 'linewidth', 1.5)
    hold on 
    plot(t,Z_measure(i+2*n+s, :), 'linestyle', '--', 'color', 'b', 'linewidth', 2)
    grid on
    ylabel(sprintf('节点电压相角_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('节点电压相角真实值与测量值 \delta', 'fontsize', 12)
    legend(sprintf('节点电压相角真实值{%d, Actual} ',i), sprintf('节点电压相角测量值{%d, EKF}', i));
    
    % ------------------test-------------------

    % ------------------test-------------------


end 

%% dynamic system model
%  USE ode45 to solve this differential equation 
function dx = dynamic_system(~,x,M,D, Ybusm,Vo,Pm,NumG)  % dynamics of the sin control system
Vg = Vo.*exp(1j*x(1:NumG));
Ibus = Ybusm*Vg;
S = conj(Ibus).*Vg;
Pe = real(S);
dx = zeros(2*NumG,1);
dx(1:NumG) = x(NumG+1:2*NumG);
dx(NumG+1:2*NumG) = (Pm-Pe)./M-D.*x(NumG+1:2*NumG)./M;
end 